/*
 * KatrinaPanel.cpp
 *
 *  Created on: 6 May 2017
 *      Author: yiming
 */

#include <katrina_gui/KatrinaPanel.h>

namespace Katrina
{
namespace Visualization
{
KatrinaPanel::KatrinaPanel(QWidget* parent) :
    rviz::Panel(parent), move_base_client_("/move_base", true), in_cruise_mode_(false), in_charge_mode_(
                                                                                            false)
{

    QVBoxLayout* main_layout = new QVBoxLayout;

    QHBoxLayout* button_layout = new QHBoxLayout;

    QPushButton* localize_button = new QPushButton(QObject::trUtf8("自动定位"));
    main_layout->addWidget(localize_button);
    addLine(main_layout);

    QPushButton* add_location_button = new QPushButton(QObject::trUtf8("添加当前位置"));
    button_layout->addWidget(add_location_button);

    QPushButton* remove_location_button = new QPushButton(QObject::trUtf8("移除选中位置"));
    button_layout->addWidget(remove_location_button);

    main_layout->addLayout(button_layout);
    addLine(main_layout);

    QVBoxLayout* known_location_layout = new QVBoxLayout;

    QLabel* known_location_label = new QLabel(QObject::trUtf8("已知位置列表"));
    known_location_layout->addWidget(known_location_label);

    location_box_ = new QComboBox;
    known_location_layout->addWidget(location_box_);
    main_layout->addLayout(known_location_layout);

    addLine(main_layout);

    QPushButton* go_button = new QPushButton(QObject::trUtf8("导航至选中位置"));
    main_layout->addWidget(go_button);
    QPushButton* stop_button = new QPushButton(QObject::trUtf8("停止"));
    main_layout->addWidget(stop_button);

    addLine(main_layout);
    QHBoxLayout* cruise_button_layout = new QHBoxLayout;
    QLabel* cruise_label = new QLabel(QObject::trUtf8("巡航路径"));
    cruise_button_layout->addWidget(cruise_label);
    cruise_remove_button_ = new QPushButton(QObject::trUtf8("移除末端点"));
    cruise_button_layout->addWidget(cruise_remove_button_);
    QPushButton* cruise_clear_button = new QPushButton(QObject::trUtf8("清除路径"));
    cruise_button_layout->addWidget(cruise_clear_button);

    main_layout->addLayout(cruise_button_layout);
    addLine(main_layout);
    location_widget_ = new QListWidget;
    //            location_widget_->setEnabled(false);
    main_layout->addWidget(location_widget_);

    QHBoxLayout* cruise_save_load_layout = new QHBoxLayout;
    cruise_open_button_ = new QPushButton(QObject::trUtf8("载入巡航路径"));
    cruise_save_load_layout->addWidget(cruise_open_button_);
    cruise_save_button_ = new QPushButton(QObject::trUtf8("保存巡航路径"));
    cruise_save_load_layout->addWidget(cruise_save_button_);
    main_layout->addLayout(cruise_save_load_layout);
    addLine(main_layout);

    QPushButton* cruise_button = new QPushButton(QObject::trUtf8("开始巡航"));
    main_layout->addWidget(cruise_button);
    addLine(main_layout);

    QHBoxLayout* status_layout = new QHBoxLayout;
    QLabel* status_label = new QLabel(QObject::trUtf8("状态"));
    status_label->setStyleSheet("QLabel { font-size: 20px;}");
    status_layout->addWidget(status_label);
    status_line_ = new QLineEdit();
    status_line_->setAlignment(Qt::AlignCenter);
    status_line_->setEnabled(false);
    status_layout->addWidget(status_line_);
    setRobotStatus(NavStatus::IDLE);
    main_layout->addLayout(status_layout);
    main_layout->addStretch();

    QHBoxLayout* battery_layout = new QHBoxLayout;
    QLabel* battery_label = new QLabel(QObject::trUtf8("电池"));
    battery_layout->addWidget(battery_label);
    battery_bar_ = new QProgressBar;
    battery_layout->addWidget(battery_bar_);
    main_layout->addLayout(battery_layout);

    QHBoxLayout* charge_layout = new QHBoxLayout;
    QLabel* auto_charge_label = new QLabel(QObject::trUtf8("自动充电"));
    charge_layout->addWidget(auto_charge_label);
    auto_charge_button_ = new QRadioButton;
    charge_layout->addWidget(auto_charge_button_);
    manual_charge_button_ = new QPushButton(QObject::trUtf8("立刻充电"));
    charge_layout->addWidget(manual_charge_button_);
    manual_charge_button_->setEnabled(false);
    auto_charge_button_->setChecked(true);
    main_layout->addLayout(charge_layout);

    setLayout(main_layout);

    connect(localize_button, SIGNAL(clicked()), this, SLOT(localizeCallback()));
    connect(add_location_button, SIGNAL(clicked()), this, SLOT(addLocationCallback()));
    connect(remove_location_button, SIGNAL(clicked()), this, SLOT(removeLocationCallback()));
    connect(go_button, SIGNAL(clicked()), this, SLOT(goToLocationCallback()));
    connect(stop_button, SIGNAL(clicked()), this, SLOT(stopCallback()));
    connect(cruise_remove_button_, SIGNAL(clicked()), this, SLOT(removeCruiseCallback()));
    connect(cruise_clear_button, SIGNAL(clicked()), this, SLOT(clearCruise()));
    connect(cruise_open_button_, SIGNAL(clicked()), this, SLOT(openCruiseFile()));
    connect(cruise_save_button_, SIGNAL(clicked()), this, SLOT(saveCruiseFile()));
    connect(cruise_button, SIGNAL(clicked()), this, SLOT(startCruising()));
    connect(auto_charge_button_, SIGNAL(clicked()), this, SLOT(autoChargeButtonCallback()));
    connect(manual_charge_button_, SIGNAL(clicked()), this, SLOT(manualautoChargeButtonCallback()));

    location_manager_.reset(new LocationManager);
    if (!nh_.getParam("/Katrina/Navigation/KnownLocationFile", location_file_path_))
        ROS_ERROR("Failed to find known location map file param");
    else
    {
        try
        {
            location_manager_->loadLocations(location_file_path_);
        }
        catch (std::exception &e)
        {
            ROS_ERROR_STREAM(e.what());
        }
    }

    for (auto&it : location_manager_->getLocations())
    {
        location_box_->addItem(QString::fromStdString(it.first));
    }

    location_mark_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/Katrina/Navigation/KnownLocations", 2,
                                                                        true);
    publishLocationsToRviz();

    cruise_number_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/Katrina/Navigation/CruisePathNumber",
                                                                        2, true);
    cruise_path_pub_ = nh_.advertise<visualization_msgs::Marker>("/Katrina/Navigation/CruisePath", 2, true);
    nav_stop_pub_ = nh_.advertise<actionlib_msgs::GoalID>("/move_base/cancel", 1, true);
    odom_sub_ = nh_.subscribe("/odom", 10, &KatrinaPanel::odomCallback, this);
    move_base_status_sub_ = nh_.subscribe("/move_base/result", 10, &KatrinaPanel::moveBaseResultCallback, this);
    mouse_cruise_location_sub_ = nh_.subscribe("/Katrina/MouseCruiseLocation", 10,
                                               &KatrinaPanel::mouseCruiseLocationCallback, this);
    self_localization_sub_ = nh_.subscribe("/Katrina/SelfLocalization", 10,
                                           &KatrinaPanel::selfLocalizationCallback, this);

    cruise_path_mark_.type = cruise_path_mark_.LINE_STRIP;
    cruise_path_mark_.action = cruise_path_mark_.ADD;
    cruise_path_mark_.header.frame_id = "map";
    cruise_path_mark_.points.clear();
    cruise_path_mark_.pose.orientation.w = 1;
    cruise_path_mark_.color.g = cruise_path_mark_.color.r = cruise_path_mark_.color.a = 1;
    cruise_path_mark_.scale.x = cruise_path_mark_.scale.y = cruise_path_mark_.scale.z = 0.05;
    cruise_number_mark_.markers.clear();
    cruise_path_.clear();

    battery_sub_ = nh_.subscribe("/battery_level", 10, &KatrinaPanel::batteryCallback, this);

    start_charge_pub_ = nh_.advertise<std_msgs::Bool>("/start_docking", 2, false);
    finish_charge_sub_ = nh_.subscribe("/finish_docking", 10, &KatrinaPanel::chargeFinishCallback, this);
    application_start_pub_ = nh_.advertise<std_msgs::Bool>("/start_application", 2, false);
    application_finish_sub_ = nh_.subscribe("/finish_application", 10, &KatrinaPanel::finishApplicationCallback,
                                            this);
    stopCallback();
}

KatrinaPanel::~KatrinaPanel()
{
    location_manager_->saveLocations(location_file_path_);
}

void KatrinaPanel::localizeCallback()
{
    if (QMessageBox::question(this, "Confirmation", QObject::trUtf8("点击OK开始自动定位，　机器人将原地旋转。"),
                              QMessageBox::Yes | QMessageBox::No) == QMessageBox::StandardButton::No)
        return;

    std::string sh = ros::package::getPath("katrina_navigation") + "/./scripts/self_localization.sh &";
    system(sh.c_str());
    setRobotStatus(NavStatus::SELF_LOCALIZATION);
}

void KatrinaPanel::getCurrentLocation(geometry_msgs::Pose &pose)
{
    tf::TransformListener listener;
    tf::StampedTransform transform;
    try
    {
        listener.waitForTransform("/map", "/base_link", ros::Time::now(), ros::Duration(2));
        listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s", ex.what());
        ros::Duration(1.0).sleep();
        QMessageBox::information(this, "Error", QString::fromStdString(ex.what()));
        return;
    }
    geometry_msgs::Transform tt;
    tf::transformTFToMsg(transform, tt);
    pose.position.x = tt.translation.x;
    pose.position.y = tt.translation.y;
    pose.position.z = tt.translation.z;
    pose.orientation.x = tt.rotation.x;
    pose.orientation.y = tt.rotation.y;
    pose.orientation.z = tt.rotation.z;
    pose.orientation.w = tt.rotation.w;
}

void KatrinaPanel::addLocationCallback()
{
    geometry_msgs::Pose new_pose;
    getCurrentLocation(new_pose);
    bool ok;
    std::string name =
            QInputDialog::getText(this, "Add new location", "Name:", QLineEdit::Normal, "", &ok).toStdString();
    if (!ok)
        return;
    addLocation(new_pose, name);
}

void KatrinaPanel::addMouseLocationCallback()
{

}

void KatrinaPanel::removeLocationCallback()
{
    std::string name = location_box_->currentText().toStdString();
    if (QMessageBox::question(this, QString::fromUtf8("确认"),
                              QString::fromUtf8("确定要移除位置 [") + QString::fromStdString(name + "] ?"),
                              QMessageBox::Yes | QMessageBox::No) == QMessageBox::StandardButton::Yes)
    {
        location_manager_->removeLocation(name);
        location_box_->removeItem(location_box_->currentIndex());
        publishLocationsToRviz();
    }
}

void KatrinaPanel::goToLocationCallback()
{
    std::string location_name = location_box_->currentText().toStdString();
    bool find_location = false;
    KnownLocation target_location;
    for (auto&it : location_manager_->getLocations())
    {
        if (location_name == it.first)
        {
            find_location = true;
            target_location = it.second;
            break;
        }
    }
    if (!find_location)
    {
        ERROR("Location "<<location_name<<" not found");
        return;
    }
    moveToLocation(target_location);
}

void KatrinaPanel::moveToLocation(const KnownLocation &location)
{
    setRobotStatus(NavStatus::INPROGRESS);
    static int id = 0;
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.header.seq = id;
    id++;
    goal.target_pose.pose = location.location;

    if (!move_base_client_.waitForServer(ros::Duration(5.0)))
        ERROR("Can not connect to move base server");
    else
    {
        move_base_client_.sendGoal(goal);
    }
}

void KatrinaPanel::stopCallback()
{
    actionlib_msgs::GoalID stop_msg;
    nav_stop_pub_.publish(stop_msg);
    HIGHLIGHT_NAMED("Navigation", "Stopping");
    if (in_cruise_mode_)
        this->clearCruise();
}

void KatrinaPanel::odomCallback(const nav_msgs::OdometryConstPtr &msg)
{
    odom_ = *msg;
}

bool KatrinaPanel::addLocation(const geometry_msgs::Pose &pose, const std::string &name)
{
    if (location_manager_->knowLocation(name))
    {
        QMessageBox::StandardButton reply;
        if (QMessageBox::question(this, "Confirmation",
                                  QString::fromStdString(name) + QString::fromUtf8(" 已经存在，点击确认更新。"),
                                  QMessageBox::Yes | QMessageBox::No) == QMessageBox::StandardButton::No)
            return false;
    }
    else
        location_box_->addItem(QString::fromStdString(name));
    location_manager_->addLocation(name, pose);

    publishLocationsToRviz();
    return true;
}

void KatrinaPanel::publishLocationsToRviz()
{
    location_marks_.markers.clear();
    for (auto&it : location_manager_->getLocations())
    {
        visualization_msgs::Marker new_mark;
        new_mark.header.frame_id = "map";
        new_mark.header.stamp = ros::Time::now();
        new_mark.id = location_marks_.markers.size();
        new_mark.action = new_mark.ADD;
        new_mark.type = new_mark.TEXT_VIEW_FACING;
        new_mark.pose = it.second.location;
        new_mark.pose.position.z = 0.15;
        new_mark.text = it.first;
        new_mark.scale.z = 0.5;
        new_mark.color.r = new_mark.color.a = 1;
        location_marks_.markers.push_back(new_mark);
    }
    location_mark_pub_.publish(location_marks_);
}

void KatrinaPanel::addLine(QBoxLayout* layout)
{
    QFrame *line = new QFrame;
    line->setFrameShape(QFrame::HLine);
    line->setFrameShadow(QFrame::Sunken);
    line->setLineWidth(1);
    layout->addWidget(line);
}

void KatrinaPanel::setRobotStatus(const NavStatus &status)
{
    switch (status)
    {
    case NavStatus::IDLE:
        status_line_->setText(QObject::trUtf8("闲置"));
        status_line_->setStyleSheet(
                    "QLineEdit { background: rgb(0, 255, 255); color: rgb(0, 0, 0); font-size: 20px;}");
        break;
    case NavStatus::INPROGRESS:
        status_line_->setText(QObject::trUtf8("巡航中"));
        status_line_->setStyleSheet(
                    "QLineEdit { background: rgb(255, 0, 255); color: rgb(0, 0, 0); font-size: 20px;}");
        break;
    case NavStatus::SUCCESS:
        //                    status_line_->setText(QObject::trUtf8("巡航完成"));
        //                    status_line_->setStyleSheet(
        //                            "QLineEdit { background: rgb(0, 255, 0); color: rgb(0, 0, 0); font-size: 20px;}");
        status_line_->setText(QObject::trUtf8("闲置"));
        status_line_->setStyleSheet(
                    "QLineEdit { background: rgb(0, 255, 255); color: rgb(0, 0, 0); font-size: 20px;}");
        break;
    case NavStatus::FAILED:
        status_line_->setText(QObject::trUtf8("巡航失败"));
        status_line_->setStyleSheet(
                    "QLineEdit { background: rgb(255, 0, 0); color: rgb(0, 0, 0); font-size: 20px;}");
        break;
    case NavStatus::SELF_LOCALIZATION:
        status_line_->setText(QObject::trUtf8("自动定位中"));
        status_line_->setStyleSheet(
                    "QLineEdit { background: rgb(100, 100, 100); color: rgb(255, 0, 0); font-size: 20px;}");
        break;
    case NavStatus::WAIT_APPLICATION:
        status_line_->setText(QObject::trUtf8("等待应用响应"));
        status_line_->setStyleSheet(
                    "QLineEdit { background: rgb(100, 100, 100); color: rgb(255, 255, 0); font-size: 20px;}");
        break;
    default:
        status_line_->setText(QObject::trUtf8("未知"));
        status_line_->setStyleSheet(
                    "QLineEdit { background: rgb(255, 0, 0); color: rgb(0, 0, 0); font-size: 20px;}");
        break;
    }
}

void KatrinaPanel::addToCruiseCallback(const KnownLocation &location)
{
    if (cruise_path_.size() == 0)
    {
        KnownLocation current_loction;
        getCurrentLocation(current_loction.location);

        visualization_msgs::Marker new_mark;
        new_mark.header.frame_id = "map";
        new_mark.header.stamp = ros::Time::now();
        new_mark.id = cruise_number_mark_.markers.size();
        new_mark.action = new_mark.ADD;
        new_mark.type = new_mark.TEXT_VIEW_FACING;
        new_mark.pose = current_loction.location;
        new_mark.pose.position.z = 0.5;
        new_mark.pose.orientation.w = 1;
        new_mark.text = std::to_string(cruise_number_mark_.markers.size());
        new_mark.scale.z = 0.5;
        new_mark.color.g = new_mark.color.b = new_mark.color.a = 1;
        cruise_number_mark_.markers.push_back(new_mark);
        geometry_msgs::Point pp;
        pp = current_loction.location.position;
        pp.z = 0.2;
        cruise_path_mark_.points.push_back(pp);
    }
    cruise_path_.push_back(location);
    geometry_msgs::Point p;
    p = location.location.position;
    p.z = 0.2;
    cruise_path_mark_.points.push_back(p);

    visualization_msgs::Marker new_mark;
    new_mark.header.frame_id = "map";
    new_mark.header.stamp = ros::Time::now();
    new_mark.id = cruise_number_mark_.markers.size();
    new_mark.action = new_mark.ADD;
    new_mark.type = new_mark.TEXT_VIEW_FACING;
    new_mark.pose.position = p;
    new_mark.pose.position.z = 0.5;
    new_mark.pose.orientation.w = 1;
    new_mark.text = std::to_string(cruise_number_mark_.markers.size());
    new_mark.scale.z = 0.5;
    new_mark.color.g = new_mark.color.b = new_mark.color.a = 1;
    cruise_number_mark_.markers.push_back(new_mark);

    cruise_path_mark_.header.stamp = ros::Time::now();

    cruise_number_pub_.publish(cruise_number_mark_);
    cruise_path_pub_.publish(cruise_path_mark_);
    location_widget_->addItem(QString::fromStdString(location.name));
}

void KatrinaPanel::removeCruiseCallback()
{
    if (cruise_path_mark_.points.size() > 0)
    {
        cruise_number_mark_.markers[cruise_number_mark_.markers.size() - 1].action =
                cruise_number_mark_.markers[cruise_number_mark_.markers.size() - 1].DELETE;
        cruise_path_mark_.points.pop_back();
        cruise_number_pub_.publish(cruise_number_mark_);
        cruise_path_pub_.publish(cruise_path_mark_);
        cruise_number_mark_.markers.pop_back();
        if (cruise_path_.size() > 0)
        {
            location_widget_->removeItemWidget(location_widget_->item(cruise_path_.size() - 1));
            cruise_path_.pop_back();
        }
    }
}

void KatrinaPanel::openCruiseFile()
{
    std::string filename = QFileDialog::getOpenFileName(
                this, "Open Cruise File",
                QString::fromStdString(ros::package::getPath("katrina_gui") + "/resources/"),
                "Document files (*.txt)").toStdString();
    if (filename != "")
    {
        if (!loadCruisePath(filename))
            ROS_ERROR_STREAM("Failed to load cruise path from "<<filename);
    }
}

bool KatrinaPanel::saveCruiseFile()
{
    if (cruise_path_.size() == 0)
    {
        ROS_ERROR("No cruise to save");
        return false;
    }
    std::string prefix = ros::package::getPath("katrina_gui") + "/resources/";
    bool ok = false;
    std::string filename = QInputDialog::getText(this, QString::fromUtf8("保存巡航路径"),
                                                 QString::fromUtf8("文件路径: ") + QString::fromStdString(prefix),
                                                 QLineEdit::Normal, "", &ok).toStdString();
    filename = prefix + filename + ".txt";
    if (ok && filename != "")
    {
        if (!checkFileExist(filename)
                || QMessageBox::question(this, QString::fromUtf8("确认"), QString::fromUtf8("文件已经存在，点击确认覆盖。"),
                                         QMessageBox::Yes | QMessageBox::No)
                == QMessageBox::StandardButton::Yes)
        {
            std::ofstream cruise_file(filename);
            if (!cruise_file.is_open())
            {
                ERROR("Fail to open location file "<<filename);
                return false;
            }
            HIGHLIGHT("Saving cruise path to file "<<filename);
            for (int i = 0; i < cruise_path_.size(); i++)
            {
                cruise_file << cruise_path_[i].name << std::endl;
            }
            cruise_file.close();
        }
    }
    return true;
}

bool KatrinaPanel::loadCruisePath(const std::string &file)
{
    std::string line;
    std::ifstream cruise_file(file);
    if (!cruise_file.is_open())
        return false;

    clearCruise();

    while (getline(cruise_file, line))
    {
        if (!location_manager_->knowLocation(line))
        {
            ROS_ERROR_STREAM("Unknown cruise location "<<line);
            return false;
        }
        KnownLocation new_location;
        new_location.name = line;
        location_manager_->getLocation(new_location.name, new_location.location);
        addToCruiseCallback(new_location);
    }
    cruise_file.close();
    HIGHLIGHT_NAMED("Navigation", "Successfully loaded cruise path from "<<file);
    return true;
}

void KatrinaPanel::startCruising()
{
    if (cruise_path_.size() == 0)
    {
        QMessageBox::information(this, "Error", QString::fromUtf8("没有找到巡航路径。"));
        return;
    }
    for (int i = 0; i < cruise_path_.size(); i++)
    {
        location_widget_->item(i)->setText(
                    QString::fromStdString(cruise_path_[i].name) + QString::fromUtf8("    等待"));
        location_widget_->item(i)->setBackgroundColor(QColor::fromRgb(200, 200, 0, 255));
    }
    current_cruise_index_ = 0;
    in_cruise_mode_ = true;
    goToNextCruiseLocation();
}

void KatrinaPanel::cancelGoal()
{
    ROS_INFO_STREAM("cancel_goal");
    move_base_client_.cancelAllGoals();
}

void KatrinaPanel::moveBaseResultCallback(const move_base_msgs::MoveBaseActionResultConstPtr &msg)
{
    ROS_INFO_STREAM("Result "<<msg->status);
    if (msg->status.status == msg->status.SUCCEEDED && in_charge_mode_)
    {
        ROS_INFO_STREAM("current goal name:"<<msg->status.text);
        ROS_INFO_STREAM("current status :"<<msg->status);
        charge();
    }
    if (!in_cruise_mode_)
    {
        setRobotStatus(NavStatus::IDLE);
    }
    else if (msg->status.status == msg->status.SUCCEEDED)
    {
        std_msgs::Bool start_msg;
        start_msg.data = true;
        application_start_pub_.publish(start_msg);
        setRobotStatus(NavStatus::WAIT_APPLICATION);

        if (current_cruise_index_ > 0)
        {
            location_widget_->item(current_cruise_index_ - 1)->setText(
                        QString::fromStdString(cruise_path_[current_cruise_index_ - 1].name)
                    + QString::fromUtf8("    已抵达"));
            location_widget_->item(current_cruise_index_ - 1)->setBackgroundColor(
                        QColor::fromRgb(200, 200, 200, 255));
        }
    }
    else
    {
        setRobotStatus(NavStatus::FAILED);
        ROS_ERROR_STREAM("Failed to cruise to location "<<cruise_path_[current_cruise_index_-1].name);
    }
}

void KatrinaPanel::finishApplicationCallback(const std_msgs::BoolConstPtr &msg)
{
    if (current_cruise_index_ < cruise_path_.size())
    {
        system("rosservice call /move_base/clear_costmaps");
        ROS_INFO_STREAM("costmaps clear ");
        goToNextCruiseLocation();
    }
    else
    {
        location_widget_->item(current_cruise_index_ - 1)->setText(
                    QString::fromStdString(cruise_path_[current_cruise_index_ - 1].name)
                + QString::fromUtf8("    已抵达"));
        location_widget_->item(current_cruise_index_ - 1)->setBackgroundColor(
                    QColor::fromRgb(200, 200, 200, 255));
        HIGHLIGHT_NAMED("Navigation", "Cruising task completed.");
        setRobotStatus(NavStatus::SUCCESS);
        clearCruise();
        in_cruise_mode_ = false;
        //std::string sh = ros::package::getPath("katrina_gui") + "/./scripts/clear_costmaps.sh &";
    }
}

void KatrinaPanel::goToNextCruiseLocation()
{
    location_widget_->item(current_cruise_index_)->setText(
                QString::fromStdString(cruise_path_[current_cruise_index_].name) + QString::fromUtf8("    巡航中"));
    location_widget_->item(current_cruise_index_)->setBackgroundColor(QColor::fromRgb(0, 255, 0, 255));
    moveToLocation(cruise_path_[current_cruise_index_]);
    current_cruise_index_++;
}

void KatrinaPanel::mouseCruiseLocationCallback(const geometry_msgs::PoseStampedConstPtr &msg)
{
    if (addLocation(msg->pose, msg->header.frame_id))
    {
        KnownLocation new_location;
        new_location.location = msg->pose;
        new_location.name = msg->header.frame_id;
        addToCruiseCallback(new_location);
    }
}

void KatrinaPanel::clearCruise()
{
    for (int i = 0; i < cruise_number_mark_.markers.size(); i++)
        cruise_number_mark_.markers[i].action = cruise_number_mark_.markers[i].DELETE;
    cruise_number_pub_.publish(cruise_number_mark_);
    cruise_path_mark_.points.clear();
    cruise_path_pub_.publish(cruise_path_mark_);
    cruise_number_mark_.markers.clear();
    if (cruise_path_.size() > 0)
        cruise_path_.pop_back();
    cruise_path_.clear();
    location_widget_->clear();
    in_cruise_mode_ = false;
}

void KatrinaPanel::selfLocalizationCallback(const std_msgs::BoolConstPtr &msg)
{
    setRobotStatus(NavStatus::IDLE);
}

void KatrinaPanel::autoChargeButtonCallback()
{
    if (auto_charge_button_->isChecked())
    {
        autoChargeEnable =  true;
        manual_charge_button_->setEnabled(false);
    }
    else
    {
        autoChargeEnable = false;
        manual_charge_button_->setEnabled(true);
    }
}

void KatrinaPanel::manualautoChargeButtonCallback()
{
    //in_autoCharge_ = true;
    this->preCharge();
}

void KatrinaPanel::preCharge()
{
    location_manager_->getLocation("ChargingStation", charge_station_.location);
    in_charge_mode_ = true;
    ROS_INFO_STREAM("Go to ChargingStation at "<<charge_station_.location);
    moveToLocation(charge_station_);
}

void KatrinaPanel::charge()
{
    std_msgs::Bool msg;
    msg.data = true;
    start_charge_pub_.publish(msg);
    HIGHLIGHT("Start charging");
}

void KatrinaPanel::batteryCallback(const std_msgs::Float32MultiArrayConstPtr &msg)
{
    if(!in_charge_mode_)
    {

        charge_flag = false;
        if (msg->data[0] != 0)
        {
            in_autoCharge_ = true;
            battery_bar_->setValue(100);
        }
        else if (msg->data[1] != 0)
        {
            if(!in_cruise_mode_ && autoChargeEnable && in_autoCharge_)
            {
                charge_flag = true;
            }
            battery_bar_->setValue(66);
        }
        else if (msg->data[2] != 0)
        {
            battery_bar_->setValue(33);
        }
        else
            battery_bar_->setValue(0);
        if (charge_flag)
        {
            HIGHLIGHT("Low battery, go to charge station");
            charge_flag = false;
            this->stopCallback();
            this->preCharge();
            in_autoCharge_ = false;
        }
    }
}

void KatrinaPanel::chargeFinishCallback(const std_msgs::BoolConstPtr &msg)
{
    if(msg->data == true)
    {
        HIGHLIGHT("Charging done");
        in_charge_mode_ = false;
    }
}
}
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(Katrina::Visualization::KatrinaPanel, rviz::Panel)
